#include "Fanuc/FanucCollector.h"
#include <cstring>
#include <thread>
#include "SocketObj/Socketobj.h"
unsigned char fanucCon1[] = {
    0xa0, 0xa0, 0xa0, 0xa0, 0x00, 0x01,
    0x01, 0x01, 0x00, 0x02, 0x00, 0x02};

unsigned char fanucCon2[] = {
    0xa0, 0xa0, 0xa0, 0xa0, 0x00, 0x01, 0x21, 0x01,
    0x00, 0x1e, 0x00, 0x01, 0x00, 0x1C, 0x00, 0x01,
    0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

unsigned char requestData[] = {
    0xa0, 0xa0, 0xa0, 0xa0, 0x00, 0x01, 0x21, 0x01,
    0x00, 0x1e, 0x00, 0x01, 0x00, 0x1c, 0x00, 0x02,
    0x00, 0x01, 0x80, 0x01, 0x00, 0x00, 0x00, 0x22,
    0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x01,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

FanucCollector::FanucCollector(/* args */) : 
    _client(nullptr)
{
    mQuit.store(false);
}

FanucCollector::~FanucCollector()
{
    mQuit.store(true);
    if (_client)
    {
        delete _client;
        _client = nullptr;
    }
}

void FanucCollector::init(const FANUC::fanucConnectInfo &info)
{
    _conInfo = info;
    packRequestData();
}

void FanucCollector::start()
{
    while (!mQuit.load())
    {
        auto ret = connectToHost();
        if (!ret)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(5000));
            continue;
        }
        startCollect();
    }
}

bool FanucCollector::connectToHost()
{
    if (_client)
    {
        try
        {
            _client = new SocketObj(_conInfo.strIp, _conInfo.nPort);

            size_t len = sizeof(fanucCon1) / sizeof(fanucCon1[0]);
            _client->sendData(fanucCon1, len);

            //这个长度需要通过返回值进行计算，先写成1024
            char buffer[1024];
            memset(buffer, 0, sizeof(buffer));
            _client->recvData(buffer, sizeof(buffer));

            len = sizeof(fanucCon2) / sizeof(fanucCon2[0]);
            _client->sendData(fanucCon2, len);

            memset(buffer, 0, sizeof(buffer));
            _client->recvData(buffer, sizeof(buffer));
        }
        catch (...)
        {
            if (_client)
            {
                delete _client;
                _client = nullptr;
            }
            return false;
        }
    }
    return true;
}

void FanucCollector::startCollect()
{
    while (!mQuit.load())
    {
        try
        {
            _client->sendData(&requestArr, requestArr.size());
            unsigned char buffer[1024];
            _client->recvData(buffer, sizeof(buffer));
        }
        catch (...)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(3));
            continue;
        }
    }
}

void FanucCollector::packRequestData()
{
    // 采集F0.7
    // 采集F26
    unsigned char hex[4];
    int addr = 0.7;
    memcpy(hex, &addr, 4);
    for (int index = 0; index < 4; index++)
    {
        requestData[23 - index] = hex[index];
        requestData[27 - index] = hex[index];
    }
    std::copy(requestData, requestData + sizeof(requestData) / sizeof(requestData[0]), std::back_inserter(requestArr));

    addr = 26;
    memcpy(hex, &addr, 4);
    for (int index = 0; index < 4; index++)
    {
        requestData[23 - index] = hex[index];
    }
    addr += 3;
    memcpy(hex, &addr, 4);
    for (int index = 0; index < 4; index++)
    {
        requestData[27 - index] = hex[index];
    }
    std::copy(requestData, requestData + sizeof(requestData) / sizeof(requestData[0]), std::back_inserter(requestArr));
}

void FanucCollector::stop()
{
    mQuit.store(true);
}